﻿using ROSBridgeLib;
using ROSBridgeLib.sensor_msgs;
using ROSBridgeLib.std_msgs;
using ROSBridgeLib.geometry_msgs;
using ROSBridgeLib.nav_msgs;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using SimpleJSON;

public class CurrentGoalSubscriber : BaseSubscriber {

	// Use this for initialization


	void Start () {


        topic="move_base_simple/goal";

        messageType="geometry_msgs/PoseStamped";

        GetComponent<SpriteRenderer>().enabled=false;

	}
	



    public override void OnReset()
    {
        base.OnReset();
        GetComponent<SpriteRenderer>().enabled=false;
    }

    public override ROSBridgeMsg Parse(JSONNode msg)
    {
       
        return new PoseStampedMsg(msg["msg"]);

    }
    public override void OnMessage()
    {
        
        PoseStampedMsg postStamp=(PoseStampedMsg)GetTopParsedPackage();

        if (postStamp==null)
        {
            return;
           
        }


        transform.localPosition=postStamp.pose.position.ToVector3();

        transform.localRotation=postStamp.pose.orientation.ToQuaternion();

        GetComponent<SpriteRenderer>().enabled=true;

    }



    public override void OnNavFinish()
    {

        GetComponent<SpriteRenderer>().enabled=false;

    }

}
